/*
 * Perspective.c
 *
 *  Created on: 2022閿熸枻鎷�10閿熸枻鎷�10閿熸枻鎷�
 *      Author: 0
 */

#include "zf_common_headfile.h"

// 逆透视映射图像,建立与原始图像的映射
uint8_t *PerImg_ip[RESULT_ROW][RESULT_COL];

// 逆透视转换矩阵

// height: 30cm
//double change_un_Mat[3][3] ={{0.265243,-0.201959,11.380245},{0.002561,0.014748,6.226394},{0.000083,-0.002168,0.232572}};
//double change_un_Mat[3][3] ={{0.257563,-0.216900,15.010979},{0.006600,0.010924,5.981987},{0.000062,-0.002244,0.259337}};

double change_un_Mat[3][3] ={{0.344576,-0.270396,17.406700},{0.003571,0.016480,4.821863},{0.000119,-0.002902,0.329862}};

// 逆透视初始化,将PerImg_ip数组与摄像头图像建立地址映射
void ImagePerspective_Init (void)
{
    static uint8_t BlackColor = 0;

    for (int i = 0; i < RESULT_COL; i++)
    {
        for (int j = 0; j < RESULT_ROW; j++)
        {
            int local_x = (int) ((change_un_Mat[0][0] * i + change_un_Mat[0][1] * j + change_un_Mat[0][2])
                    / (change_un_Mat[2][0] * i + change_un_Mat[2][1] * j + change_un_Mat[2][2]));
            int local_y = (int) ((change_un_Mat[1][0] * i + change_un_Mat[1][1] * j + change_un_Mat[1][2])
                    / (change_un_Mat[2][0] * i + change_un_Mat[2][1] * j + change_un_Mat[2][2]));
            if (local_x >= 0 && local_y >= 0 && local_y < USED_ROW && local_x < USED_COL)
            {
                PerImg_ip[j][i] = &PER_IMG[local_y][local_x];
            }
            else
            {
                PerImg_ip[j][i] = &BlackColor;          //&PER_IMG[0][0];
            }
        }
    }
}

// 坐标逆透视,将在原始图像上的坐标转换为逆透视后的坐标
uint8 ImagePerspective (int x, int y, float *x1, float *y1)
{
    float x11, y11;

    float a1 = change_un_Mat[2][0] * y - change_un_Mat[1][0];
    float b1 = change_un_Mat[2][1] * y - change_un_Mat[1][1];
    float c1 = -(change_un_Mat[2][2] * y - change_un_Mat[1][2]);
    float a2 = change_un_Mat[2][0] * x - change_un_Mat[0][0];
    float b2 = change_un_Mat[2][1] * x - change_un_Mat[0][1];
    float c2 = -(change_un_Mat[2][2] * x - change_un_Mat[0][2]);

    x11 = (b1 * c2 - b2 * c1) / (b1 * a2 - b2 * a1);
    y11 = (c1 * a2 - c2 * a1) / (b1 * a2 - b2 * a1);

    if (x11 > 0 && y11 > 0 && y11 < RESULT_ROW && x11 < RESULT_COL)
    {
        *x1 = (float) x11;
        *y1 = (float) y11;
        return 0;
    }
    else
    {
        // 转换失败
        return 1;
    }
}

// 坐标反逆透视,将在逆透视图像上的坐标转换为原图的坐标
void inv_Perspective (float x, float y, int *x1, int *y1)
{
    int local_x = (int) ((change_un_Mat[0][0] * x + change_un_Mat[0][1] * y + change_un_Mat[0][2])
            / (change_un_Mat[2][0] * x + change_un_Mat[2][1] * y + change_un_Mat[2][2]));
    int local_y = (int) ((change_un_Mat[1][0] * x + change_un_Mat[1][1] * y + change_un_Mat[1][2])
            / (change_un_Mat[2][0] * x + change_un_Mat[2][1] * y + change_un_Mat[2][2]));

    *x1 = (int) local_x;
    *y1 = (int) local_y;
}
